%%% start "real-time" EKF 
close all

%%% handle to function to map observation data
fhObs = @(X,Y,cog,sog,rot)[X; Y; ...
    unwrap(cog * pi / 180);...
    sog;...
    rot*pi/180];

%%% configure AIS Data Caster
AISDPublisher = DataPublisher(...
    '-fname','aisTestData.mat',...
    '-vesselID',211436320,...
    '-dtPrediction',1,...
    '-iFacRT',1,...
    '-bConstantPredictionRate',true,...
    '-hObsMapping',fhObs);

%%% load batch AIS data (from file); flag bControlIn steers use of control
%%% input
AISDPublisher.loadAISdata(...
            '-istart',300,...
            '-iend',2300);

%%% state dimensions
stateDim_ = 5;
%%%%%%%%%%%%%%%%%%%%%
%%% process model %%%
%%%%%%%%%%%%%%%%%%%%%
%%% state transition
f_ = @(dt,state) [...
    state(1) + dt * state(4) * sin(state(3) + dt*state(5)); ...
    state(2) + dt * state(4) * cos(state(3) + dt*state(5)); ...
    state(3) + dt*state(5); ...
    state(4); ...
    state(5)];
%%% Jacobian w.r.t. to state
JacobianF_ = @(dt,state)[ ...
    1 0 dt*state(4)*cos(state(3) + dt*state(5)) dt*sin(state(3) + dt*state(5)) dt^2*state(4)*cos(state(3) + dt*state(5));  ...
    0 1 -dt*state(4)*sin(state(3) + dt*state(5)) dt*cos(state(3) + dt*state(5)) -dt^2*state(4)*sin(state(3) + dt*state(5)); ...
    0 0 1 0 dt; ...
    0 0 0 1 0; ...
    0 0 0 0 1];
%%% Jacobian w.r.t. process noise
JacobianC_ = @(dt,state)[...
    dt*sin(state(3) + dt*state(5)) dt^2*state(4)*cos(state(3) + dt*state(5)); ...
    dt*cos(state(3) + dt*state(5)) -dt^2*state(4)*sin(state(3) + dt*state(5)); ...
    0 dt; ...
    1 0; ...
    0 1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% measurement model (taking only positions) %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h_ = @(obsIn)[obsIn(1); obsIn(2)];
%%% Jacobian of h() w.r.t. state
JacobianH_ = [1 0 0 0 0; 0 1 0 0 0];
%%% sigmas process noise (constituting Q)
sVel_ = .07;
sOmega_ = .1/180*pi;
%%% process noise covariance matrix
Q_ = @(~) diag([sVel_^2, sOmega_^2]);
%%% measurement noise covariance matrix
R_ = diag([5, 5].^2);
%%% initial error covariance matrix
P_ = eye(stateDim_);
%%% define indexes of parameters to be put into observation model
idxObsModel = [1 2];
%%% define indexes of parameters to be put into process model
idxProcModel = (1:5);
%%% time interval for prediction step (in s)
dt = 1;
%%% we could define some stopping criterion (i.e. elapsed time)
% tMax = 5000

cYLabels = {'p_x [m]', 'p_y [m]', '\psi [rad]','v [m/s]','\omega [rad/s]'};
cTitles = {'position estimate x-axis', 'position estimate y-axis', 'COG estimate','SOG estimate','ROT estimate' };

ExtendedKalmanFilter(...
    '-masterObj',AISDPublisher,...
    '-dynModel',f_,...
    '-measModel',h_,...
    '-FJ',JacobianF_,...
    '-CJ',JacobianC_,...
    '-HJ',JacobianH_,...
    '-R',R_,...
    '-Q',Q_, ...
    '-P',P_,...
    '-labelsTitles',cTitles,...
    '-labelsYAxes',cYLabels,...
    '-state', AISDPublisher.getInitialState(idxProcModel));

AISDPublisher.setObsIdxs(idxObsModel);

%%% start real-world simulation for integrity check; timer objects will be
%%% started for asynchronous prediction and update steps
AISDPublisher.startDataProcessing();

